#include <iostream>
#include "map.h"
#include "graph.h"
#include "types.h"
#include "los.h"
#include "astar.h"
#include "mpi.h"

int main(int argc, char** argv)
{
  MPI::Init(argc, argv);
  int num_p = MPI::COMM_WORLD.Get_size();
  int id    = MPI::COMM_WORLD.Get_rank();

  std::cout << "Running on " << id << " of " << num_p << std::endl;

  Map myMap;
  if (id == 0)
  {
    // Processor 0 will read the initial map data and broadcast it to
    // everyone else
    std::cout << "Processor " << id << " reading input file..." << std::endl;
    myMap.readFile("Test.lvl");
    // TODO: Broadcast map to other processors
    int mapSize[] = {myMap.width(), myMap.height()};
    MPI::COMM_WORLD.Bcast(mapSize, 2, MPI::INT, 0);
    MPI::COMM_WORLD.Bcast(const_cast<int*>(myMap.data()), myMap.size(), MPI::INT, 0);
  }
  else
  {
    // All other processors will wait until processor 0 tells them the map
    // size and data
    int mapSize[2];
    MPI::COMM_WORLD.Bcast(mapSize, 2, MPI::INT, 0);
    int size = mapSize[0]*mapSize[1];
    int* data = new int[size];
    MPI::COMM_WORLD.Bcast(data, size, MPI::INT, 0);
    myMap.setData(mapSize[0], mapSize[1], data);
  }

  Graph graph;
  //myMap.print(std::cout);

  // Reduce the map to only corners and add those corners to the graph.
  int interval = myMap.size()/num_p;
  int nodes_per_interval = interval/2;  // Assume max interval/2 possible nodes per interval
  int start = id * interval;
  int end = start + interval - 1;
  if (id == num_p-1)
    ++end;  // include the last cell on the last processor.

  // Find all corners in the map within the specified interval
  getCorners(myMap, graph, start, end);

  // Get the nodes from the graph and add them to a single array
  const NodeMap& nodes = graph.getNodes();
  int* sendData = new int[nodes_per_interval];
  memset(sendData, 0, sizeof(int)*nodes_per_interval);
  sendData[0] = nodes.size();
  int idx = 1;
  NodeMap::const_iterator citr = nodes.begin();
  for (; citr != nodes.end(); ++citr)
  {
    sendData[idx++] = citr->second->getIndex().row;
    sendData[idx++] = citr->second->getIndex().col;
  }

  // Gather all corners to processor 0
  int* recvData = 0;

  if (id == 0)
    recvData = new int[num_p*nodes_per_interval];

  MPI::COMM_WORLD.Gather(sendData, nodes_per_interval, MPI::INT,
                         recvData, nodes_per_interval, MPI::INT,
                         0);

  if (id == 0)
  {
    // Add graph nodes from other processors to local graph
    // (Skip first range since it's the nodes from own graph)
    for (int i = 1; i < num_p; ++i)
    {
      idx = i*nodes_per_interval;

      int numNodes = recvData[idx++];
      for (int j = 0; j < numNodes; ++j)
      {
        int row = recvData[idx++];
        int col = recvData[idx++];
        NodeIndex recvIndex(col, row);
        graph.addNode(recvIndex);
        //std::cout << "Received index " << recvIndex << std::endl;
      }
    }

    // done with recvData
    delete [] recvData;

    NodeIndex startIndex(0, 6);
    NodeIndex goalIndex(14, 1);

    // NOTE: This will only be done by processor 0
    // Add the start and goal nodes to the graph
    graph.addNode(NodeIndex(startIndex.x, startIndex.y));
    graph.addNode(NodeIndex(goalIndex.x, goalIndex.y));

    // Calculate the edges of the graph using the existing map to find
    // visibility between nodes.
    graph.calculateEdges(myMap);

    // DEBUG: Output nodes and edges
    const NodeMap& nodes = graph.getNodes();
    NodeMap::const_iterator citr = nodes.begin();
    std::cout << "Edges in graph:" << std::endl;
    for (; citr != nodes.end(); ++citr)
    {
      Node* node = citr->second;
      std::cout << node->getIndex() << " has the following neighbors:" << std::endl;
      const NodeMap& neighbors = node->getNeighbors();
      NodeMap::const_iterator citr2 = neighbors.begin();
      for (; citr2 != neighbors.end(); ++citr2)
        std::cout << "\t" << citr2->second->getIndex() << std::endl;
      std::cout << std::endl;
    }

    Path path;
    astar(graph, startIndex, goalIndex, path);
    std::cout << "Path to goal:" << std::endl;
    for (size_t i = 0; i < path.size(); ++i)
      std::cout << "\t" << path[i] << std::endl;
  }

  MPI::Finalize();
  return 0;
}
